/*
 * @Author: your name
 * @Date: 2021-06-03 21:46:02
 * @LastEditTime: 2021-07-28 20:26:41
 * @LastEditors: Please set LastEditors
 * @Description: 主要是与四个寄存器相关的函数
 * @FilePath: \TEST\Core\mycode\encoder.c
 */
#include "encoder.h"
#include "math.h"
#include "stdio.h"
#include "float_transmit.h"
#include "usart.h"
float circu =47.123; //车轮的周长，换算成厘米，半径为7.5cm
//编码器得到的计数
float num_per_turn = 1560;//每圈计数的次数
float period = 50;//计数的周期的倒数，这里现在用的是20ms的周期
int  encoder_distence[4] = {0,0,0,0};
float encoder_speed[4] = {0,0,0,0};
/**
 * @description: 读取一次轮子1编码器的值，读完后将数字写回为32768
 * @param {*}
 * @return {*}
 */
uint16_t read_encoder1(void)
{
    uint16_t num = TIM2->CNT; 
    TIM2->CNT = (uint16_t)ENCODER_COUNTER_START_NUM;
    return num;
}

/**
 * @description: 读取一次轮子2编码器的值，读完后将数字写回为32768
 * @param {*}
 * @return {*}
 */
uint16_t read_encoder2(void)
{
    uint16_t num = TIM3->CNT;
    TIM3->CNT = (uint16_t)ENCODER_COUNTER_START_NUM;
    return num;
}

/**
 * @description: 读取一次轮子3编码器的值，读完后将数字写回为32768
 * @param {*}
 * @return {*}
 */
uint16_t read_encoder3(void)
{
    uint16_t num = TIM4->CNT; 
    TIM4->CNT = (uint16_t)ENCODER_COUNTER_START_NUM;
    return num;
}

/**
 * @description: 读取一次轮子4编码器的值，读完后将数字写回为32768
 * @param {*}
 * @return {*}
 */
uint16_t read_encoder4(void)
{
    uint16_t num = TIM5->CNT; 
    TIM5->CNT = (uint16_t)ENCODER_COUNTER_START_NUM;
    return num;
}

/**
 * @description: 
 * @param {*}
 * @return {*}
 */

int* get_derta_encoder_num(int dater_num_max)
{
    static int dater_encoder_num[4];
    dater_encoder_num[0] = (read_encoder1() - (uint16_t)ENCODER_COUNTER_START_NUM);
    dater_encoder_num[1] = (read_encoder2() - (uint16_t)ENCODER_COUNTER_START_NUM);
    dater_encoder_num[2] = (read_encoder3() - (uint16_t)ENCODER_COUNTER_START_NUM);
    dater_encoder_num[3] = (read_encoder4() - (uint16_t)ENCODER_COUNTER_START_NUM);
    for(int i=0;i<4;i++)
    {
        if (abs(dater_encoder_num[i])<= dater_num_max)
        {
        encoder_distence[i] = encoder_distence[i] + dater_encoder_num[i];
        // float a = (float)encoder_distence[i];
        // float_transmit(&huart2, &a, 1);
        }
        
    }
    return dater_encoder_num;
}
/**
 * @description: 获取某个轮子的速度，主要看输入的encoder_num_now为哪个编码器的，由上面的read_encodern来读取
 * @param {uint16_t} encoder_num_now读到的编码器
 * @param {uint16_t} v_max 当得到的数据大于最大速度时被忽略
 * dan_or_shuang 1表示为1，3两个轮子，0表示2，4两个轮子
 * @return {*}
 */
float get_speed(float v_max)
{
    int* a = get_derta_encoder_num(1000);
//    float speed = dater_encoder_num/num_per_turn*circu*period;
    for ( int i = 0; i < 4; i++)
    {
        float speed= (float)(*(a+i))/num_per_turn*360.0*period;
        if (fabs(speed) <= v_max )
        {
            encoder_speed[i] = speed;
        }
        else
        {
            encoder_speed[i] =  encoder_speed[i];
        }
    }
}






